using UnityEngine;

using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.IO;
using System.Text; 
using IMIForUnity;

public class ImiDollController : MonoBehaviour {	
	// Bool that has the characters (facing the player) actions become mirrored. Default true.
	public bool MirroredMovement = true;

    public bool JointSmoothFilter = true;

    public bool BindMainPlayer = true;

    // Bool that determines whether the player will move or not in space.
    private bool MovesInSpace = false;

    // Bool that determines whether the player is allowed to jump -- vertical movement
	// can cause some models to behave strangely, so use at your own discretion.
    private bool VerticalMovement = false;

    // Rate at which player will move through the scene. The rate multiplies the movement speed (.001f, i.e dividing by 1000, unity's framerate).
    private int MoveRate = 1;
	
	// Slerp smooth factor
    public float SmoothFactor = 20.0f;

    private ImiSkeleton[] g_skeleton;

	// Public variables that will get matched to bones. If empty, the kinect will simply not track it.
	// These bones can be set within the Unity interface.
    public Transform Head;
    public Transform Neck;
    public Transform Spine;
    public Transform HipCenter;

    public Transform LeftShoulder;
    public Transform LeftElbow;
    public Transform LeftWrist;
    public Transform LeftHand;

    public Transform RightShoulder;
    public Transform RightElbow;
    public Transform RightWrist;
    public Transform RightHand;

    public Transform LeftHip;
    public Transform LeftKnee;
    public Transform LeftAnkle;
    public Transform LeftFoot;

    public Transform RightHip;
    public Transform RightKnee;
    public Transform RightAnkle;
    public Transform RightFoot;

    public Transform Root;

    // A required variable if you want to rotate the model in space.
    public GameObject offsetNode;

    // Variable to hold all them bones. It will initialize the same size as initialRotations.
    private Transform[] bones;

    // Rotations of the bones when the Kinect tracking starts.
    private Quaternion[] initialRotations;

    // Calibration Offset Variables for Character Position.
    private bool OffsetCalibrated = false;
    private float XOffset, YOffset, ZOffset;
    private Quaternion originalRotation;

    private ImiJointConstraint jointconstraint;

    public void Start() 
    {
		// ToDo
    }

    public void Init()
    {
        jointconstraint = new ImiJointConstraint();

        // Holds our bones for later.
        bones = new Transform[(int)ImiSkeleton.Index.COUNT];

        // Initial rotations of said bones.
        initialRotations = new Quaternion[(int)ImiSkeleton.Index.COUNT];

        // Map bones to the points the Kinect tracks.
        MapBones();

        // Get initial rotations to return to later.
        GetInitialRotations();

        // Set the model to the calibration pose.
        RotateToCalibrationPose(false);
    }

    public void LateUpdate() 
    {
        if (ImiManager.GetInstance().IsDeviceWorking())
        {
            ProcessSkeleton();
        }
	}

    // Process the skeleton data
    private void ProcessSkeleton()
    {
        ImiPlayerInfo mInfo = null;
        if (BindMainPlayer)
        {
            mInfo = ImiManager.GetInstance().GetMainPlayerInfo();
        }
        else
        {
            mInfo = ImiManager.GetInstance().GetSubPlayerInfo();
        }

        if (mInfo != null && mInfo.GetPlayerTracked() && mInfo.userId > 0
			&& mInfo.skeletons[(int)ImiSkeleton.Index.HIP_CENTER].isTracked)
        {
            g_skeleton = mInfo.skeletons;

			if (g_skeleton[(int)ImiSkeleton.Index.FOOT_LEFT].isTracked
				&& g_skeleton[(int)ImiSkeleton.Index.FOOT_RIGHT].isTracked
				&& g_skeleton[(int)ImiSkeleton.Index.HIP_CENTER].isTracked)
            {
                UpdatePlayer(false);
            }
            else
            {
                UpdatePlayer(true);
            }

        } else {
            RotateToCalibrationPose(false);
        }
    }

    // Update the player each frame.
    private void UpdatePlayer(bool IsNearMode) 
    {
        Utils.isMirror = MirroredMovement;
        jointconstraint.isJointSmoothFilter = JointSmoothFilter;

        // Update Head, Neck, Spine, and Hips normally.
        TransformBone(ImiSkeleton.Index.HIP_CENTER, 0);
		TransformBone(ImiSkeleton.Index.SPINE, 1);
		TransformBone(ImiSkeleton.Index.SHOULDER_CENTER, 2);
		TransformBone(ImiSkeleton.Index.HEAD, 3);

		TransformBone(ImiSkeleton.Index.SHOULDER_LEFT, MirroredMovement ? 8 : 4);
		TransformBone(ImiSkeleton.Index.ELBOW_LEFT, MirroredMovement ? 9 : 5);
		TransformBone(ImiSkeleton.Index.WRIST_LEFT, MirroredMovement ?  10 : 6);
		TransformBone(ImiSkeleton.Index.HAND_LEFT, MirroredMovement ?  11 : 7);

		TransformBone(ImiSkeleton.Index.SHOULDER_RIGHT, MirroredMovement ? 4 : 8);
		TransformBone(ImiSkeleton.Index.ELBOW_RIGHT, MirroredMovement ? 5 : 9);
		TransformBone(ImiSkeleton.Index.WRIST_RIGHT, MirroredMovement ? 6 : 10);
		TransformBone(ImiSkeleton.Index.HAND_RIGHT, MirroredMovement ? 7 : 11);    

        if (!IsNearMode)
		{
			TransformBone(ImiSkeleton.Index.HIP_LEFT, MirroredMovement ? 16 : 12);
			TransformBone(ImiSkeleton.Index.KNEE_LEFT, MirroredMovement ? 17 : 13);
			TransformBone(ImiSkeleton.Index.ANKLE_LEFT, MirroredMovement ? 18 : 14);
			TransformBone(ImiSkeleton.Index.FOOT_LEFT, MirroredMovement ? 19 : 15);
			
			TransformBone(ImiSkeleton.Index.HIP_RIGHT, MirroredMovement ? 12 : 16);
			TransformBone(ImiSkeleton.Index.KNEE_RIGHT, MirroredMovement ? 13 : 17);
			TransformBone(ImiSkeleton.Index.ANKLE_RIGHT, MirroredMovement ? 14 : 18);
			TransformBone(ImiSkeleton.Index.FOOT_RIGHT, MirroredMovement ? 15 : 19);	
		}

        // If the player is supposed to move in the space, move it.
		if (MovesInSpace) {
			MovePlayer();
		}
    }

    // Apply the rotations tracked by the kinect to the joints.
    private void TransformBone(ImiSkeleton.Index joint, int boneIndex)
    {
        Transform boneTransform = bones[boneIndex];
        if (boneTransform == null)
        {
            return;
        }

        // Get joint orientation
        Quaternion jointRotation;
        if (BindMainPlayer)
        {
            jointRotation = ImiManager.GetInstance().GetMainPlayerInfo().GetPlayerJointsOri()[(int)joint];
        }
        else
        {
            jointRotation = ImiManager.GetInstance().GetSubPlayerInfo().GetPlayerJointsOri()[(int)joint];
        }

        // Apply the new rotation.
        Quaternion newRotation = jointRotation * initialRotations[boneIndex];

        //If an offset node is specified, combine the transform with its
        //orientation to essentially make the skeleton relative to the node
        if (offsetNode != null)
        {
            // Grab the total rotation by adding the Euler and offset's Euler.
            Vector3 totalRotation = newRotation.eulerAngles + offsetNode.transform.rotation.eulerAngles;
            // Grab our new rotation.
            newRotation = Quaternion.Euler(totalRotation);
        }

        // Smoothly transition to our new rotation.
        boneTransform.rotation = jointconstraint.updateJointOrientation(boneTransform.rotation, newRotation, SmoothFactor, g_skeleton, joint);
    }

    // Moves the player in 3D space - pulls the tracked position of the spine and applies it to root.
    // Only pulls positional, not rotational.
    private void MovePlayer()
    {
        if (Root == null || Root.parent == null)
        {
            return;
        }

        // Get the position of the body and store it.
        Vector3 playerPosition;
        if (BindMainPlayer)
        {
            playerPosition = ImiManager.GetInstance().GetMainPlayerInfo().GetPlayerPosition();
        } else
        {
            playerPosition = ImiManager.GetInstance().GetSubPlayerInfo().GetPlayerPosition();
        }

        // If this is the first time we're moving the player, set the offset. Otherwise ignore it.
        if (!OffsetCalibrated)
        {
            OffsetCalibrated = true;

            XOffset = !MirroredMovement ? playerPosition.x * MoveRate : -playerPosition.x * MoveRate;
            YOffset = playerPosition.y * MoveRate;
            ZOffset = -playerPosition.z * MoveRate;
        }

        float xPos, yPos, zPos;

        // If movement is mirrored, reverse it.
        if (!MirroredMovement)
        {
            xPos = playerPosition.x * MoveRate - XOffset;
        }
        else
        {
            xPos = -playerPosition.x * MoveRate - XOffset;
        }

        yPos = playerPosition.y * MoveRate - YOffset;
        zPos = -playerPosition.z * MoveRate - ZOffset;

        // If we are tracking vertical movement, update the y. Otherwise leave it alone.
        Vector3 targetPos = new Vector3(xPos, VerticalMovement ? yPos : 0f, zPos);
        Root.parent.localPosition = Vector3.Lerp(Root.parent.localPosition, targetPos, 3 * Time.deltaTime);
    }
	
	// Set bones to initial position.
    private void RotateToInitialPosition() 
    {	
		// For each bone that was defined, reset to initial position.
		for (int i = 0; i < bones.Length; i++) {
			if (bones[i] != null) {
				bones[i].rotation = initialRotations[i];
			}
		}
    }
	
	// Calibration pose is simply initial position with hands raised up. Rotation must be 0,0,0 to calibrate.
    private void RotateToCalibrationPose(bool needCalibration) 
    {	
		if(needCalibration) {
			// Reset the rest of the model to the original position.
	        RotateToInitialPosition();
			
			// Right Elbow
			if(RightElbow != null)
                RightElbow.rotation = Quaternion.Euler(0, -90, 90) *
                       initialRotations[(int)ImiSkeleton.Index.ELBOW_RIGHT];


            // Left Elbow
            if (LeftElbow != null)
                LeftElbow.rotation = Quaternion.Euler(0, 90, -90) *
                    initialRotations[(int)ImiSkeleton.Index.ELBOW_LEFT];
		}
    }
	
	// If the bones to be mapped have been declared, map that bone to the model.
	private void MapBones() 
    {
        if (HipCenter != null)
            bones[0] = HipCenter;
        if (Spine != null)
            bones[1] = Spine;
        if (Neck != null)
            bones[2] = Neck;
        if (Head != null)
			bones[3] = Head;
		
		if(LeftShoulder != null)
			bones[4] = LeftShoulder;
		if(LeftElbow != null)
			bones[5] = LeftElbow;
		if(LeftWrist != null)
			bones[6] = LeftWrist;
		if(LeftHand != null)
			bones[7] = LeftHand;

		if(RightShoulder != null)
			bones[8] = RightShoulder;
		if(RightElbow != null)
			bones[9] = RightElbow;
		if(RightWrist != null)
			bones[10] = RightWrist;
		if(RightHand != null)
			bones[11] = RightHand;

		if(LeftHip != null)
			bones[12] = LeftHip;
		if(LeftKnee != null)
			bones[13] = LeftKnee;
		if(LeftAnkle != null)
			bones[14] = LeftAnkle;
		if(LeftFoot != null)
			bones[15] = LeftFoot;
		
		if(RightHip != null)
			bones[16] = RightHip;
		if(RightKnee != null)
			bones[17] = RightKnee;
		if(RightAnkle != null)
			bones[18] = RightAnkle;
		if(RightFoot!= null)
			bones[19] = RightFoot;
	}
	
	// Capture the initial rotations of the model.
	private void GetInitialRotations() 
    {
		if(offsetNode != null) {
			// Store the original offset's rotation.
			originalRotation = offsetNode.transform.rotation;
			// Set the offset's rotation to 0.
			offsetNode.transform.rotation = Quaternion.Euler(Vector3.zero);
        } 
		
		for (int i = 0; i < bones.Length; i++) {
			if (bones[i] != null) {
				initialRotations[i] = bones[i].rotation;
			}
		}

		if(offsetNode != null) {
			// Restore the offset's rotation
			offsetNode.transform.rotation = originalRotation;
		} 
	}
}